#ifndef _MOTOR_H_
#define _MOTOR_CONTROL_H_

#include "common.h"

#define INTERRUPT1_MOTOR_CONTROL_TIME   20

#define LEFT_MOTOR_CONTROL_P               28
#define LEFT_MOTOR_CONTROL_I               0.2
#define LEFT_MOTOR_CONTROL_D               0.0
#define LEFT_MOTOR_CONTROL_INTEGRAL_LIMIT  1000
#define LEFT_MOTOR_CONTROL_MAX_OUTPUT      6000

#define RIGHT_MOTOR_CONTROL_P               0//28
#define RIGHT_MOTOR_CONTROL_I               0
#define RIGHT_MOTOR_CONTROL_D               0.0
#define RIGHT_MOTOR_CONTROL_INTEGRAL_LIMIT  1000
#define RIGHT_MOTOR_CONTROL_MAX_OUTPUT      6000

#define FORWARD_VEL -80
#define FORWARD_P   30
#define FORWARD_I   0
#define FORWARD_D   0.0
#define FORWARD_INTEGRAL_LIMIT  1200
#define FORWARD_MAX_OUTPUT      2000

typedef struct _MOTOR_CONTROL_t
{
    PID_t left_motor_control_pid;
    PID_t right_motor_control_pid;
    PID_t forward_pid;
    float left_motor_output,right_motor_output,forward_output;
    int32 left_motor_count;
    int32 right_motor_count;
    
    int16 left_current_vel;
    int16 right_current_vel;
    int16 forward_current_vel;
    
    int16 left_target_vel;
    int16 right_target_vel;
    int16 forward_target_vel;
    void (*f_init_motor_control)(struct _MOTOR_CONTROL_t *motor_control);

}MOTOR_CONTROL_t;

void MOTOR_CONTROL_StructInit(MOTOR_CONTROL_t *motor_control);

#endif